%Constantes de la planta

g=9.81;
Mr=0.03;
Mb=0.7;
H=0.144;
L=H/2;
R=0.028;
Jw=Mr*R*R/2;
W=0.14;
D=0.04;
Jf=Mb*L*L/3;
Jt=Mb*(W*W+D*D)/12;
Jm=0.00001;
Rm=6.69;
Kb=0.468;
Kt=0.317;
n=1;
fm=0.0022;
fw=0;

%Ecuaciones
e11 = (2*Mr+Mb)*R*R + 2*Jw+2*n*n*Jm;
e12 = Mb*L*R-2*n*n*Jm;
e21 = Mb*L*R-2*n*n*Jm;
e22 = Mb*L*L+Jf+2*n*n*Jm;

alfa=n*Kt/Rm;
beta=n*Kt*Kb/Rm+fm;

detE = e11*e22-e12*12;

%Ecuaciones de estado
a32 = -g*Mb*L*e12/detE;
a42 = g*Mb*L*e11/detE;
a33 = -2*((beta+fw)*e22+beta*e12)/detE;
a43 = 2*((beta+fw)*e12+beta*e11)/detE;
a34 = 2*beta*(e22+e12)/detE;
a44 = -2*beta*(e11+e12)/detE;
b3 = alfa*(e22+e12)/detE;
b4 = -alfa*(e11+e12)/detE;


A=[0 0 1 0;
    0 0 0 1;
    0 a32 a33 a34;
    0 a42 a43 a44];
B=[0;
    0;
    b3
    b4];

C=[0 1 0 0;
    0 0 0 1;
    1 0 0 0;
    0 0 1 0];

D=[0;
    0;
    0;
    0];

%Matrices para los pesos de LQR
Q=[50 0 0 0; 0 50 0 0; 0 0 1 0; 0 0 0 1];

R=1;

[K,S,e] = LQR(A,B,Q,R);